// SEE END OF FILE FOR LICENSE TERMS

#include "telemetryManager.h"
#include "dataIDrefs.h"

boolean requestFastTelemetry = false;
boolean requestSlowTelemetry = false;

unsigned char fastTelemetryIndex = 0;
unsigned char slowTelemetryIndex = 0;


// Get the next telemetry identifier in the queue
//  Return ID_NULL if finished sending
unsigned char get_next_telemetry_identifier(void);


// Copy the next data chunk into the telemetry data structure
// If the data is finished, return false.  If the data is good return true
// also write the number of bytes written to the passed variable
boolean get_next_data_chunk(TELEMETRY_DATA* pteleData);


// Variables for tracking the transmittion of array variables
// required for when the array is larger than the data packet can cope with
// in the case of CANbus the packet is 8 bytes max.
unsigned char 	currentIDRef 		= ID_NULL;		// The idRef variable being transmitted
unsigned int  	currentArrayOffset 	= 0;			// The current array offset being transmitted


// Set the request for fast telemetry data to be sent
void requestFastTelemetrySend(void)
{
	requestFastTelemetry = true;
}

// Set the request for slow telemetry data to be sent
void requestSlowTelemetrySend(void)
{
	requestFastTelemetry = true;
};


// Get the next telemetry data
// return true if data is filled, return false if data send finished
boolean get_next_telemetry_data(TELEMETRY_DATA* pteleData)
{
	// Check if it we are at the start of transmission
	// If so, get the next required idRef.
	if(currentIDRef == ID_NULL)
	{
		currentIDRef = get_next_telemetry_identifier();
		if(currentIDRef == ID_NULL) return false;	// return if nothing needed

		if(get_next_data_chunk(pteleData) == false) return false;
	}
	else
	{
		// if at the end of this data chunk, get the next one
		if(get_next_data_chunk(pteleData) == false)
		{
			currentIDRef = get_next_telemetry_identifier();
			// if finshed then leave and do nothing
			if(currentIDRef == ID_NULL) return false;			// return if nothing needed
	
			if(get_next_data_chunk(pteleData) == false) return false;
		};
	};

	return true;
};


// Get the next telemetry identifier in the queue
//  Return ID_NULL if finished sending
unsigned char get_next_telemetry_identifier()
{
	unsigned char identifier;

	currentArrayOffset 	= 0;	// Reset the array offset

	// Check if fast telemetry is requested.  This takes priority
	if(requestFastTelemetry == true)
	{
		identifier = FastTelemetryList[fastTelemetryIndex];

		if(identifier == ID_NULL)
		{
			requestFastTelemetry = false;
			fastTelemetryIndex = 0;
		}
		else
		{
			fastTelemetryIndex++;
		}
	}

	// the fast telemetry may be finished 
	//  so the slow telemetry check needs a new statement

	if( (requestSlowTelemetry == true) &&
			 (requestFastTelemetry == false) )
	{
		identifier = SlowTelemetryList[slowTelemetryIndex];

		if(identifier == ID_NULL)
		{
			requestSlowTelemetry = false;
			slowTelemetryIndex = 0;
		}
		else
		{
			slowTelemetryIndex++;
		}
	};
	
	return identifier;
};



// Copy the next data chunk into the telemetry data structure
// If the data is finished, return false.  If the data is good return true
// also write the number of bytes written to the passed variable
boolean get_next_data_chunk(TELEMETRY_DATA* pteleData)
{
	unsigned char arrayReadSize = 0;	// The number of array elements read from the database
	unsigned char readSize 		= 0;	// The number of bytes read

	unsigned char arrayMaxSize = DBGetArraySize(currentIDRef);	// Maximum size of the data array

	unsigned char typeSize = DBGetTypeSize(currentIDRef);

	pteleData->arrayOffset = currentArrayOffset;	// Save the array offset into telemetry data
	
	while( ((readSize + typeSize) <= CANB_MAX_DATA_SIZE) && (arrayReadSize > arrayMaxSize) )
	{
		arrayReadSize += 1;
		readSize += typeSize;
		currentArrayOffset += 1;
	}

	pteleData->arraySize 	= arrayReadSize;
	pteleData->byteSize 	= readSize;

	return DBReadData(currentIDRef, pteleData->pData, arrayReadSize, pteleData->arrayOffset);
};



/****************************************************************************/
// This is part of the servo and radio interface software
//
// ServoInterface source code
//	http://code.google.com/p/rc-servo-interface
//
// Copyright 2010 ServoInterface Team
// See the AUTHORS.TXT file for a list of authors of ServoInterface.
//
// ServoInterface is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// ServoInterface is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License 
// along with ServoInterface.  If not, see <http://www.gnu.org/licenses/>.
//
// Many parts of ServoInterface use either modified or unmodified code
// from the MatrixPilot pilot project.
// The project also contains code for modifying MatrixPilot to operate
// with ServoInterface.
// For details, credits and licenses of MatrixPilot see the AUTHORS.TXT file.
// or see this website: http://code.google.com/p/gentlenav
/****************************************************************************/
